

const char* MENU_STR[] =
{
    ("Euler angles    | R    P    Y    "),    ("Servo Channel   | R=   P=   Y=   "), ("Servo Trim      | R=   P=   Y=   "),
    ("Servo Speed     | R=   P=   Y=   "),    ("Servo Reverse   | R=   P=   Y=   "), ("Servo Travel    |     L=    R=   "),
    ("PPM Mixing      | Enable=        "),    ("Radio System    | Type=          "), ("Mag Calibration | Confirm?       "), 
    ("Acc Calibration | Confirm?       "), //    ("Reset Settings  | Confirm?       ")
};

const char* CHAN_STR[] = {"ROL ", "PIT ", "YAW "};

enum MENU
{
    MENU_ATT, MENU_CHAN, MENU_TRIM,
    MENU_SPD, MENU_REV,  MENU_TRV, 
    MENU_MIX, MENU_RADIO, MENU_MAG, 
    MENU_ACC, 
    //MENU_RST,
    MENU_END
};

static int8_t menu_index = 0;
static int8_t submenu_index = -1;
static int8_t ahrs_output = 0;
static int8_t tmp_ahrs_output = 0;

static int16_t servo_filter[3];
bool filter_init = true;

static int8_t trv_tune_servo = 0;

const int8_t AHRS_OUTPUT_ALL_SERVO = 0x7;

void control_update()
{
    static uint8_t counter = 0;  //100Hz counter

    keyboard_update();
    
    switch (keyboard_get_key())
    {
    case KEY_ENTER:
        on_key_enter();
        break;
    case KEY_UP:
        on_key_up_down(true);
        break;
    case KEY_DOWN:
        on_key_up_down(false);
        break;
    }
  
    if (menu_index == MENU_ATT && counter > 1)
    {
        menu_display_data(-1);
        counter = 0;
    }
    else if (submenu_index >= 0
     && (menu_index == MENU_MAG && counter > 200 || menu_index == MENU_ACC && counter > 35))
    {
        if (!ahrs.ready())
            lcd.write('>');
        else
        {
            menu_index = 0;
            submenu_index = -1;
            keyboard_enable();
            ahrs.reset(true, true);
            ahrs_output = tmp_ahrs_output;
            menu_display_main();
        }
        counter = 0;
    }
    
//    if (ahrs_output && menu_index != MENU_TRIM
//        || menu_index == MENU_SPD || menu_index == MENU_TRV_L || menu_index == MENU_TRV_R)
    if (ahrs_output)
    {
        if (filter_init)
        {
            for (uint8_t i = 0; i < 3; i++)
                servo_filter[i] = ahrs.euler_angle[i] << 2;
            filter_init = false;
        }
        else
        {
        	  for (uint8_t i = 0; i < 3; i++)
                servo_filter[i] = (servo_filter[i] - servo_out[i]) + ahrs.euler_angle[i];
        }
        
        for (uint8_t i = 0; i < 3; i++)
            if (ahrs_output & (1 << i))    
                servo_out[i] = servo_filter[i] >> 2;
    }

    ++counter;
}

void on_key_enter()
{
//    static bool ahrs_enable = false;

    submenu_index++;

    switch (menu_index)
    {
    case MENU_ATT:
        //acc.calibrate();
        ahrs.set_zero();
//        ahrs.enable = true;
        submenu_index = -1;
        ahrs_output = AHRS_OUTPUT_ALL_SERVO;
        filter_init = true;
        break;
    case MENU_TRIM:
        if (submenu_index == 0)
        {
            servo_out[0] = servo_out[1] = servo_out[2] = 0;
            tmp_ahrs_output = ahrs_output;
            ahrs_output = 0;
        }
        else if (submenu_index > 2)
        {
            ahrs_output = tmp_ahrs_output;
            filter_init = true;
            submenu_index = -1;
        }
        break;
    case MENU_REV:
    case MENU_CHAN:
    //case MENU_TRV_L:
    //case MENU_TRV_R:
    case MENU_SPD:
        if (submenu_index == 0)
        {
            tmp_ahrs_output = ahrs_output;
            ahrs_output = AHRS_OUTPUT_ALL_SERVO;
            filter_init = true;
        }
        else if (submenu_index > 2)
        {
            ahrs_output = tmp_ahrs_output;
            submenu_index = -1;
        }
        break;
    case MENU_TRV:
    	  if (submenu_index == 0)
    	  {
    	  	  tmp_ahrs_output = ahrs_output;
    	  }
    	  else if (submenu_index == 1)
    	  {
    	  	  ahrs_output = trv_tune_servo;
    	  	  filter_init = true;
    	  }
    	  else if (submenu_index > 2)
        {
            ahrs_output = tmp_ahrs_output;
            submenu_index = -1;
        }
        break;
    case MENU_MIX:
    case MENU_RADIO:
        if (submenu_index > 0)
            submenu_index = -1;
        break;
    case MENU_ACC:
    case MENU_MAG:
        ahrs.calibrate(menu_index == MENU_ACC? IMU_ACC: IMU_MAG);
        keyboard_disable();
        tmp_ahrs_output = ahrs_output;
        ahrs_output = 0;
        lcd.set_pos(0, 1);
        lcd.print("         ");
        lcd.set_pos(0, 1);
        break;

        //
        //    case MENU_RST:
        //        PPM::reset_setting();
        //        submenu_index = 0;
        //        break;* /
    }
    menu_display_data(submenu_index);

    if (submenu_index == -1 && menu_index >= MENU_CHAN && menu_index <= MENU_RADIO)
        PPM::save_setting();
}

void on_key_up_down(bool up)
{
    if (submenu_index == -1)
    {
        if (up && --menu_index < 0)
            menu_index = MENU_END - 1;
        else if (!up && ++menu_index >= MENU_END)
            menu_index = 0;
        menu_display_main();
    }
    else
    {
        switch (menu_index)
        {
        case MENU_TRIM:
        {
            int8_t trim = PPM::get_trim(submenu_index);
            if (up) trim++;
            else    trim--;
            PPM::set_trim(submenu_index, trim);
            break;
        }
        case MENU_REV:
            PPM::change_reverse(submenu_index);
            break;
        case MENU_CHAN:
        {
            int8_t chan = ((int8_t)PPM::get_channel(submenu_index)) + (up?1:-1);
            if (chan == 1)      chan = 5;
            else if (chan < 0)  chan = 8;
            else if (chan > 8 || chan < 5)
                chan = 0;
            PPM::set_channel(submenu_index, chan);
            break;
        }
        case MENU_SPD:
        {
            uint8_t spd = PPM::get_speed(submenu_index);
            if (up) spd++;
            else if (spd > 0) spd--;
            PPM::set_speed(submenu_index,  spd);
            break;
        }
        case MENU_TRV:
            if (submenu_index == 0)
            {
                if (up && ++trv_tune_servo > 2) trv_tune_servo = 0;
                if (!up && --trv_tune_servo < 0) trv_tune_servo = 2;
                //refresh data
                menu_display_data(1);
                menu_display_data(2);
            }
            else if (submenu_index == 1)
            {
            	//Serial.printf("TRV: %i ", PPM::get_min(trv_tune_servo));
                if (up) PPM::set_min(trv_tune_servo, PPM::get_min(trv_tune_servo) - 6);
                else PPM::set_min(trv_tune_servo, PPM::get_min(trv_tune_servo) + 6);
                //Serial.printf("%i\n", PPM::get_min(trv_tune_servo));
            }
            else if (submenu_index == 2)
            {
            	  if (up) PPM::set_max(trv_tune_servo, PPM::get_max(trv_tune_servo) + 6);
                else PPM::set_max(trv_tune_servo, PPM::get_max(trv_tune_servo) - 6);
            }
            break;
        case MENU_MIX:
            PPM::change_mixing();
            break;
        case MENU_RADIO:
            PPM::change_radio_type();
            break;
        }
        menu_display_data(submenu_index);
    }
}

void menu_display_main()
{
    lcd.set_pos(0,0);
    lcd.print(MENU_STR[menu_index]);
    menu_display_data(-1);
}

void menu_display_data(int8_t submenu)
{
    if(menu_index >= MENU_ATT && menu_index < MENU_TRV)
    {
        if (submenu == -1)
        {
            for (uint8_t i = 0; i < 3; i++)
                menu_display_data(i);
            return;
        }
        else if (menu_index > MENU_ATT)
        {
            lcd.set_pos(submenu * 5 + 3, 1);
        }
    }
    
    lcd.blink(false);

    switch (menu_index)
    {
    case MENU_ATT:
        lcd.set_pos(submenu * 5 + 2, 1);
        lcd.printf("%-4i", ahrs.euler_angle[submenu]/10);
        break;
    case MENU_TRIM:
        lcd.printf("%-3i", PPM::get_trim(submenu));
        break;
    case MENU_REV:
        lcd.print(PPM::get_reverse(submenu)?"r":"n");
        break;
    case MENU_CHAN:
    {
        uint8_t c = PPM::get_channel(submenu);
        if (c)  lcd.printf("%-3i", c);
        else    lcd.print("off");
        break;
    }
    case MENU_SPD:
        lcd.printf("%-3i", PPM::get_speed(submenu));
        break;
    case MENU_TRV:
    	  if (submenu == 0)
    	  {
    	      lcd.set_pos(1, 1);
    	      lcd.print(CHAN_STR[trv_tune_servo]);
    	  }
    	  else if (submenu == 1)
          {
              lcd.set_pos(7, 1);
    	      lcd.printf("%-3i", (1520 - PPM::get_min(trv_tune_servo))/6);
          }
    	  else
          {
              lcd.set_pos(13, 1);
    	      lcd.printf("%-3i", (PPM::get_max(trv_tune_servo)-1520)/6);
          }
        break;
    case MENU_MIX:
        lcd.set_pos(8,1);
        lcd.print(PPM::get_mixing()? "on ":"off");
        if (submenu == 0)
            lcd.blink(8,1);
        break;
    case MENU_RADIO:
        lcd.set_pos(6,1);
        lcd.print(PPM::get_radio_type() == RADIO_FUTABA? "Futaba  ": "JR Propo");
        if (submenu == 0)
            lcd.blink(6,1);
        break;
    }

    if(menu_index > MENU_ATT && menu_index < MENU_MIX && submenu_index >= 0)
        lcd.blink(submenu * 5 + 3, 1);
    
    if (menu_index == MENU_TRV && submenu == 0)
    	lcd.blink(1, 1);
}


